#!/usr/bin/env python

import unittest
import roslib; roslib.load_manifest('faces')
from sensor_msgs.msg   import CameraInfo,PointCloud
from std_msgs.msg      import Header
from faces.msg         import Face,Faces
from geometry_msgs.msg import Point
from faceDetector      import FaceDetector
from sensor_msgs.msg   import Image

class FacesTests(unittest.TestCase):
    def setUp(self):
        self.subject = FaceDetector()

    def testTranslateFacePointsToFacesMessage(self):
        pointsList        = [Point(2,2,2), Point(4,4,4)]
        pointCloud        = PointCloud
        pointCloud.points = pointsList
        pointCloud.header = Header()

        expectedResults = Faces()
        expectedResults.faces.append(Face())
        expectedResults.faces[0].upperLeft  = Point(1.9619,2,2.0762)
        expectedResults.faces[0].lowerRight = Point(2.0381,2,1.9238)
        expectedResults.faces[0].center     = pointsList[0]
        expectedResults.faces.append(Face())
        expectedResults.faces[1].upperLeft  = Point(3.9619,4,4.0762)
        expectedResults.faces[1].lowerRight = Point(4.0381,4,3.9238)
        expectedResults.faces[1].center     = pointsList[1]
        expectedResults.header = pointCloud.header
        expectedResults.image = Image()

        self.assertEqual(
            self.subject.translateFacePointsToFacesMessage(
                pointCloud,
                expectedResults.image
            ),
            expectedResults
        )

def main():
    unittest.main()

if __name__ == '__main__':
    main()
